#include "virtual_camera/camera_node.hpp"

// ROS
#include <ament_index_cpp/get_package_share_directory.hpp>

// Third Party
#include <rclcpp/logging.hpp>
#include <rclcpp/time.hpp>
#include <yaml-cpp/yaml.h>

namespace virtual_camera {
    
CameraNode::CameraNode(const rclcpp::NodeOptions &options) : Node("camera_node", rclcpp::NodeOptions(options).use_intra_process_comms(true)) {
    // 在composable node中通过launch加载会失效，所以需要手动读取yaml文件
    // https://github.com/ros2/launch_ros/issues/156
    std::string yaml_name = ament_index_cpp::get_package_share_directory("virtual_camera") + "/config/camera.yml";
    YAML::Node yaml = YAML::LoadFile(yaml_name);
    image_width = yaml["image_width"].as<int>();
    image_height = yaml["image_height"].as<int>();
    video_name = yaml["video_name"].as<std::string>();
    fps = yaml["fps"].as<int>();

    // 设置参数
    image_width = this->declare_parameter<int>("image_width", image_width);
    image_height = this->declare_parameter<int>("image_height", image_height);
    video_name = this->declare_parameter<std::string>("video_name", video_name);
    fps = this->declare_parameter<int>("fps", fps);

    // 初始化相机
    camera = std::make_shared<VirtualCamera>(video_name);
    if (camera->open() != 0) {
        RCLCPP_ERROR(this->get_logger(), "视频读取失败");
        return;
    } else {
        RCLCPP_INFO(this->get_logger(), "虚拟相机启动成功");
        RCLCPP_INFO(this->get_logger(), "分辨率: %d x %d", image_width, image_height);
        RCLCPP_INFO(this->get_logger(), "视频文件: %s", video_name.c_str());
        RCLCPP_INFO(this->get_logger(), "帧率: %d", fps);
    }

    // 设置视频文件参数回调
    video_param_sub_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
    auto video_cb = [this](const rclcpp::Parameter &p) {
        video_name = p.as_string();
        camera = std::make_shared<VirtualCamera>(video_name);
        if (camera->open() != 0) {
            RCLCPP_ERROR(this->get_logger(),"更改视频失败");
            return;
        }
    };
    video_cb_handle_ = video_param_sub_->add_parameter_callback("video_name", video_cb);

    // 设置fps参数回调
    fps_param_sub_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
    auto fps_cb = [this](const rclcpp::Parameter &p) {
        fps = p.as_int();
        timer_->cancel();
        timer_ = this->create_wall_timer(std::chrono::milliseconds(1000/fps), std::bind(&CameraNode::getImage, this));
    };
    fps_cb_handle_ = fps_param_sub_->add_parameter_callback("fps", fps_cb);

    // 图像话题发布
    rclcpp::QoS img_qos(rclcpp::QoSInitialization::from_rmw(_img_qos));
    image_pub_ = this->create_publisher<sensor_msgs::msg::Image>("/image_raw", img_qos);

    // 设置定时器
    timer_ = this->create_wall_timer(std::chrono::milliseconds(1000/fps), std::bind(&CameraNode::getImage, this));
}

void CameraNode::getImage() {
    cv::Mat frame = camera->getFrame();
    rclcpp::Time stamp = this->now();
    if (frame.empty()) {
        RCLCPP_ERROR(this->get_logger(), "图像获取失败");
        return;
    }

    sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());

    image_msg->header.frame_id = "odom";
    image_msg->header.stamp = stamp;
    image_msg->height = frame.rows;
    image_msg->width = frame.cols;
    image_msg->encoding = "bgr8";
    image_msg->step = static_cast<sensor_msgs::msg::Image::_step_type>(frame.step);
    image_msg->data.resize(image_msg->step * frame.rows);
    memcpy(&image_msg->data[0], frame.data, image_msg->data.size());

    RCLCPP_DEBUG(this->get_logger(), "发出图像, 地址：%p", image_msg.get());

    image_pub_->publish(std::move(image_msg));
}

}

#include "rclcpp_components/register_node_macro.hpp"

RCLCPP_COMPONENTS_REGISTER_NODE(virtual_camera::CameraNode)